#include <LitStdAfx.h>
#include "LitPhysicsWorld.h"
#include <LitCore.h>
#include <LitSceneManager.h>
#include "LitPhysicsDebug.h"

namespace Lit{
	namespace Physics
	{
		void World::setPlatformOptimisation( PlatformOptimisation optimis ){
			NewtonSetPlatformArchitecture (*mWorld, optimis);
		}
		void World::setSolverModel( SolverModel model ){
			NewtonSetSolverModel (*mWorld, model);
		}
		World::World( const std::string &name, PlatformOptimisation optimis, SolverModel solver, const Lit::AABB& dimensions )
			: mName( name )
		{
			mFrameTime = 0.0f;
			mWorld = NewtonCreate();

			setPlatformOptimisation( optimis ); 
			setSolverModel( solver );

			if( dimensions == Lit::AABB::BOX_INFINITE )	mDimensions = dimensions;
			else setDimensions( dimensions );

			mDebug = new Debugger(*Lit::Root::SceneManager()->_sceneManager(), this);
		}
		World::~World( ){
			clear();
			if( mWorld ) 
			{
				NewtonDestroy( *mWorld );
			}
			if( mDebug ){delete mDebug, mDebug = NULL; }
		}

		void World::clear()
		{
			if( mWorld ) 
			{
				NewtonDestroyAllBodies( *mWorld );
			}
		}
	
		void World::forEachBodyDo( World::BodyIteratorType itor ){
			Body * body = getFirstBody( );
			while( body )
			{
				itor( body );
				body = getNextBody( body );
			}
		}
		Body*		World::getFirstBody(){
			if( !mWorld ) return NULL;
			NewtonBody * b = NewtonWorldGetFirstBody( *mWorld );
			if( !b ) return NULL;
			return (Body*)NewtonBodyGetUserData(b);
		}
		const Body* World::getFirstBody() const{
			if( !mWorld ) return NULL;
			NewtonBody * b = NewtonWorldGetFirstBody( *mWorld );
			if( !b ) return NULL;
			return (Body*)NewtonBodyGetUserData(b);
		}
		Body*		World::getNextBody( const Body * body ){
			if( !body || !mWorld ) return NULL;
			NewtonBody * b = NewtonWorldGetNextBody( *mWorld, body->getBody() );
			if( b )	return (Body*)NewtonBodyGetUserData(b);
			return NULL;
		}
		const Body*	World::getNextBody( const Body * body ) const{
			if( !body || !mWorld ) return NULL;
			NewtonBody * b = NewtonWorldGetNextBody( *mWorld, body->getBody() );
			if( b )	return (Body*)NewtonBodyGetUserData(b);
			return NULL;
		}
		size_t		World::bodyCount() const{ return NewtonWorldGetBodyCount(*mWorld ); }

		NewtonWorld*	World::getWorld(){ return *mWorld; }
		const NewtonWorld* World::getWorld() const{ return *mWorld; }

		void World::_resetBodyCallback( Body * b ){
			b->restoreInitialState();
		}
		void World::resetWorld( ){
			forEachBodyDo( boost::bind( &World::_resetBodyCallback, this, _1 ) );
		}
		void World::update( float dt )
		{
			mFrameTime = dt;
			if( mWorld )
			{
				NewtonUpdate(*mWorld, dt);
				if( mDebug )
					mDebug->rebuildLines();
				//mDebugger->updateObjects( *mWorld );
			}

		}
		void World::setSize( const Lit::Vec3 &size ){
			// set a fix world size
			Lit::Vec3 minSize (-size*0.5);
			Lit::Vec3 maxSize (size*0.5);
			mDimensions = Lit::AABB( minSize, maxSize );
			NewtonSetWorldSize (*mWorld, &minSize[0], &maxSize[0]); 
		}
		void World::setCorners( const Lit::Vec3 &minimum, const Lit::Vec3& maximum ){
			// set a fix world size
			Lit::Vec3 minSize (minimum);
			Lit::Vec3 maxSize (maximum);
			mDimensions = Lit::AABB( minimum, maximum );
			NewtonSetWorldSize (*mWorld, &minSize[0], &maxSize[0]); 
		}
		Lit::AABB World::dimensions() const{
			return mDimensions;
		}
		BodyConstIterator World::getConstIterator() const{
			return BodyConstIterator( this );
		}
		BodyIterator World::getIterator(){
			return BodyIterator( this );
		}

		void World::setDimensions( const Lit::AABB& box ){
			// set a fix world size
			Lit::Vec3 minSize (box.getMinimum());
			Lit::Vec3 maxSize (box.getMaximum());
			NewtonSetWorldSize (*mWorld, &minSize[0], &maxSize[0]); 
			mDimensions = box;
		}
		CollisionShape * World::_createCollision(Lit::Entity * ent, ShapeType type, int shapeId, const Lit::Vec3 &initial_pos, const Lit::Quat& initial_orient ){
			CollisionShape * shape= NULL;
			switch( type )
			{
				case ST_BOX: shape = new BoxShape( ent->getEntity()->getWorldBoundingBox(true), shapeId, Matrix( initial_pos, initial_orient ) ); break;
				case ST_SPHERE: shape = new SphereShape( ent->getEntity()->getWorldBoundingBox(true), shapeId, Matrix( initial_pos, initial_orient ) ); break;
				case ST_PLANE: shape = new PlaneShape( ent->getEntity()->getWorldBoundingBox(true), shapeId, Matrix( initial_pos, initial_orient ) ); break;
				default: shape = new NullShape();
			};
			return shape;
		}
		Body* World::createStaticBody( Lit::Entity * ent, ShapeType type, int shapeId, const Lit::Vec3 &shape_pos, const Lit::Quat& shape_orient ){
			Matrix transMat(ent->getPosition(), ent->getOrientation());
			CollisionShape * shape= _createCollision(ent, type, shapeId, shape_pos, shape_orient);
			_CollisionPtr collision = shape->createShape( mWorld );
			Lit::Vec3 minBox;
			Lit::Vec3 maxBox;
			Lit::Vec3 origin;
			Lit::Vec3 inertia;
			NewtonBody* body = NULL;
			Matrix mat2;
			// Now with the collision Shape we can crate a rigid body
			body = NewtonCreateBody (*mWorld, *collision, mat2.toNewton( ) );
 
			Body* ret = new Body( this, 0.0f, ent, body, shape );

			NewtonBodySetDestructorCallback (body, &BodyDestructorCallback);
			NewtonBodySetUserData (body, ret);

			ret->setMatrix( transMat );

			ret->defineInitialState();
			NewtonBodySetTransformCallback (body, &BodyTransformCallback);
		
			ret->_setStatic( true );
			shape->destroyShape( mWorld, *collision );

			LIT_LOG_DBG("Static RigidBody '"+ret->getName()+"' created");
			return ret;
		}
		Body* World::createBody( float mass, Lit::Entity * ent, ShapeType type, int shapeId, const Lit::Vec3 &shape_pos, const Lit::Quat& shape_orient )
		{
			Matrix transMat(ent->getPosition(), ent->getOrientation());
			CollisionShape * shape= _createCollision(ent, type, shapeId, shape_pos, shape_orient);
			_CollisionPtr collision = shape->createShape( mWorld );
			Lit::Vec3 minBox;
			Lit::Vec3 maxBox;
			Lit::Vec3 origin;
			Lit::Vec3 inertia;
			
			Matrix mat2;
			// Now with the collision Shape we can crate a rigid body
			NewtonBody * body = NewtonCreateBody (*mWorld, *collision, mat2.toNewton( ) );
 
			Body* ret = new Body( this, mass, ent, body, shape );

			NewtonBodySetDestructorCallback (body, &BodyDestructorCallback);
			NewtonBodySetUserData (body, ret);

			ret->setMatrix(transMat );
			
			InertialMatrix inertialmat(shape->calculateInertialMatrix( mass, collision ));
			ret->setInertia(inertialmat);
		
			ret->defineInitialState();

			// set function callbacks
			NewtonBodySetForceAndTorqueCallback (body, &BodyTorqueForceCallback);
			NewtonBodySetTransformCallback (body, &BodyTransformCallback);
		
			ret->_setStatic( false );
			shape->destroyShape( mWorld, *collision );
		
			LIT_LOG_DBG("RigidBody '"+ret->getName()+"' created");
			return ret;
		}
		void World::destroyBody( BodyPtr b )
		{
			if( !b ) return;
			if( mWorld && b->getBody() )
			{
				NewtonDestroyBody( *mWorld, b->getBody() );
			}
		}

		void World::BodyDestructorCallback( const NewtonBody * b )
		{
			if( b )
			{
				Body * data = (Body*)NewtonBodyGetUserData( b );
				if( data )
				{
					try
					{
						if( Lit::Root::SceneManager()->hasEntity(data->getEntity()->getName()) )
							Lit::Root::SceneManager()->removeEntity( data->getEntity()->getName() );
					}
					catch( ... )
					{}
					delete data, data = NULL;
					NewtonBodySetUserData( b, NULL );
				}
			}
		}
		void World::BodyTorqueForceCallback( const NewtonBody * b, float timestep, int threadIndex )
		{
			if( b )
			{
				Body * data = (Body*)NewtonBodyGetUserData( b );
				if( data )
				{
					try
					{
							Lit::Real Ixx;
							Lit::Real Iyy;
							Lit::Real Izz;
							Lit::Real mass;
 
							// for this tutorial the only external force in the Gravity
							NewtonBodyGetMassMatrix (b, &mass, &Ixx, &Iyy, &Izz);
							ForceList& forces(data->getExternalForces());
							Lit::Vec4 grav( .0f, mass * PHYSICS_GRAVITY, .0f, 1.0f );
							NewtonBodySetForce(b, &grav.x);

							ForceList::Result res = forces.calculateExternalForces( data );
							if( res.has_force ) NewtonBodySetForce(b, &(grav+res.force)[0]);
							if( res.has_torque ) NewtonBodySetTorque(b, &res.torque[0]);
							if( res.has_veloc ) NewtonBodySetVelocity(b, &res.veloc[0]);
					}
					catch( ... )
					{}
				}
			}
			
		}
		void World::BodyTransformCallback( const NewtonBody * b, const float * matrix, int threadIndex )
		{
			if( b && matrix )
			{
				Body * data = (Body*)NewtonBodyGetUserData( b );
				Matrix mat = Matrix::NewtonToOgre( matrix );
				if( data && data->getEntity() ){
					data->getEntity()->setOrientation( mat.getOrientation() ); 
					data->getEntity()->setPosition( mat.getPosition() ); 
				}
			/*	Lit::Vec4 posit (matrix[12], matrix[13], matrix[14], 1.0f);
				Lit::Quat rotation;
 
				// we will ignore the Rotation part of matrix and use the quaternion rotation stored in the body
				NewtonBodyGetRotation(b, &rotation.x);
 
				if( data )
				{
					// get the getEntity associated with this rigid body
					Lit::Entity* ent = data->getEntity();
					Matrix mat( Lit::Vec3(posit.x, posit.y, posit.z), rotation );
					// set the new position and orientation for this getEntity
					if( ent ){
						ent->setPosition( mat.getPosition() );
						ent->setOrientation( mat.getOrientation() );
					}
				}*/
			}
		}
	};
};